#include "ctrl.h"

#include <string.h>
#include <assert.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <errno.h>
#include <termios.h>
#include <stdarg.h>
#include <sys/time.h>
#include <stdint.h>

#include "util.h"

typedef enum PARITY {
    COMM_NOPARITY,
    COMM_ODDPARITY,
    COMM_EVENPARITY
}PARITY;

typedef enum PURGE {
    COMM_PURGE_TXABORT,
    COMM_PURGE_RXABORT,
    COMM_PURGE_TXCLEAR,
    COMM_PURGE_RXCLEAR,
}PURGE;

typedef struct COMM_ATTR {
    int baudrate;
    PARITY parity;
    int databits;
    int stopbits;
}COMM_ATTR;


#define RELAY_ON 0x01
#define RELAY_OFF 0x02
#define RELAY_COUNT 5
#define PROTO_LEN 10
#define RS485_NAME "/dev/ttyAMA1"

static int dev_fd; 

int uart_set_attribute(int dev_fd, COMM_ATTR *pattr)
{
    struct termios opt;
    COMM_ATTR *attr = pattr;
    if (dev_fd < 0)
    {
        return -1;
    }

    memset(&opt, 0, sizeof(struct termios));
    tcgetattr(dev_fd, &opt);
    cfmakeraw(&opt);            

    switch (attr->baudrate)
    {
        case 50:
            cfsetispeed(&opt, B50);
            cfsetospeed(&opt, B50);
            break;
        case 75:
            cfsetispeed(&opt, B75);
            cfsetospeed(&opt, B75);
            break;
        case 110:
            cfsetispeed(&opt, B110);
            cfsetospeed(&opt, B110);
            break;
        case 134:
            cfsetispeed(&opt, B134);
            cfsetospeed(&opt, B134);
            break;
        case 150:
            cfsetispeed(&opt, B150);
            cfsetospeed(&opt, B150);
            break;
        case 200:
            cfsetispeed(&opt, B200);
            cfsetospeed(&opt, B200);
            break;
        case 300:
            cfsetispeed(&opt, B300);
            cfsetospeed(&opt, B300);
            break;
        case 600:
            cfsetispeed(&opt, B600);
            cfsetospeed(&opt, B600);
            break;
        case 1200:
            cfsetispeed(&opt, B1200);
            cfsetospeed(&opt, B1200);
            break;
        case 1800:
            cfsetispeed(&opt, B1800);
            cfsetospeed(&opt, B1800);
            break;
        case 2400:
            cfsetispeed(&opt, B2400);
            cfsetospeed(&opt, B2400);
            break;
        case 4800:
            cfsetispeed(&opt, B4800);
            cfsetospeed(&opt, B4800);
            break;
        case 9600:
            cfsetispeed(&opt, B9600);
            cfsetospeed(&opt, B9600);
            break;
        case 19200:
            cfsetispeed(&opt, B19200);
            cfsetospeed(&opt, B19200);
            break;
        case 38400:
            cfsetispeed(&opt, B38400);
            cfsetospeed(&opt, B38400);
            break;
        case 57600:
            cfsetispeed(&opt, B57600);
            cfsetospeed(&opt, B57600);
            break;
        case 115200:
            cfsetispeed(&opt, B115200);
            cfsetospeed(&opt, B115200);
            break;
        case 230400:
            cfsetispeed(&opt, B230400);
            cfsetospeed(&opt, B230400);
            break;
        case 460800:
            cfsetispeed(&opt, B460800);
            cfsetospeed(&opt, B460800);
            break;
        case 500000:
            cfsetispeed(&opt, B500000);
            cfsetospeed(&opt, B500000);
            break;
        case 576000:
            cfsetispeed(&opt, B576000);
            cfsetospeed(&opt, B576000);
            break;
        case 921600:
            cfsetispeed(&opt, B921600);
            cfsetospeed(&opt, B921600);
            break;
        case 1000000:
            cfsetispeed(&opt, B1000000);
            cfsetospeed(&opt, B1000000);
            break;
        case 1152000:
            cfsetispeed(&opt, B1152000);
            cfsetospeed(&opt, B1152000);
            break;
        case 1500000:
            cfsetispeed(&opt, B1500000);
            cfsetospeed(&opt, B1500000);
            break;
        case 2000000:
            cfsetispeed(&opt, B2000000);
            cfsetospeed(&opt, B2000000);
            break;
        case 2500000:
            cfsetispeed(&opt, B2500000);
            cfsetospeed(&opt, B2500000);
            break;
        case 3000000:
            cfsetispeed(&opt, B3000000);
            cfsetospeed(&opt, B3000000);
            break;
        case 3500000:
            cfsetispeed(&opt, B3500000);
            cfsetospeed(&opt, B3500000);
            break;
        case 4000000:
            cfsetispeed(&opt, B4000000);
            cfsetospeed(&opt, B4000000);
            break;
        default:
            printf("unsupported baudrate %d\n", attr->baudrate);
            break;
    }
    
    switch (attr->parity)
    {
        case COMM_NOPARITY:        
            opt.c_cflag &= ~PARENB;    
            opt.c_iflag &= ~INPCK;    
            break;
        case COMM_ODDPARITY:        
            opt.c_cflag |= PARENB;    
            opt.c_cflag |= PARODD;    
            opt.c_iflag |= INPCK;    
            break;
        case COMM_EVENPARITY:        
            opt.c_cflag |= PARENB;    
            opt.c_cflag &= ~PARODD;    
            opt.c_iflag |= INPCK;    
        break    ;
        default:
            printf("unsupported parity %d\n", attr->parity);
            break;
    }

    
    opt.c_cflag &= ~CSIZE;
    switch (attr->databits)
    {
        case 5:
            opt.c_cflag |= CS5;
            break;
        case 6:
            opt.c_cflag |= CS6;
            break;
        case 7:
            opt.c_cflag |= CS7;
            break;
        case 8:
            opt.c_cflag |= CS8;
            break;
        default:
            printf("unsupported data bits %d\n", attr->databits);
            break;
    }

    
    opt.c_cflag &= ~CSTOPB;
    switch (attr->stopbits)
    {
        case 0:
        case 1:
            opt.c_cflag &= ~CSTOPB;
            break;

        case 2:
            opt.c_cflag |= CSTOPB;
            break;
        default:
            printf("unsupported stop bits %d\n", attr->stopbits);
            opt.c_cflag &= ~CSTOPB;
            break;
    }
    opt.c_cc[VTIME]    = 0;
    opt.c_cc[VMIN]    = 1;            

    tcflush(dev_fd, TCIOFLUSH);
    if (tcsetattr(dev_fd, TCSANOW, &opt) < 0)
    {
        printf("tcsetattr\n");
        return -1;
    }

    return 0;
} 

int uart_get_attribute(int dev_fd)
{
    int Uart_fd = dev_fd;
    struct termios opt;

    if (Uart_fd < 0)
    {
        return -1;
    }

    memset(&opt, 0, sizeof(struct termios));
    tcgetattr(Uart_fd, &opt);
    cfmakeraw(&opt);            

    printf("cfgetospeed(&opt): %d\n", cfgetospeed(&opt));
    printf("cfgetispeed(&opt): %d\n", cfgetispeed(&opt));


    printf("B2400 is %d, B115200 is %d, c_oflag is 0x%x\n", B2400, B115200, opt.c_oflag);

    return 0;
} 

int uart_destory(int dev_fd)
{
    int ret = -1;
    if (dev_fd >= 0)
    {
        ret = close(dev_fd);
    }

    return ret;
}

int uart_create(char *strDev)
{
    int dev_fd = -1;

    dev_fd = open(strDev, O_RDWR | O_NOCTTY | O_NDELAY);
    //dev_fd = open(strDev, O_RDWR);
    if (dev_fd < 0)
    {
        printf("FrontboardCreate Fbddev Failed\n");
        return -1;
    }
        
    printf("UARTCreate %s Successful\n",strDev);
    
    return dev_fd;
}

/*iFlag 0 阻塞，1 非阻塞*/
int uart_create_new(char *strDev,int iFlag)
{
    int dev_fd = -1;

    if (iFlag)
    {
        dev_fd = open(strDev, O_RDWR |O_NONBLOCK);
    }
    else
    {
        dev_fd = open(strDev, O_RDWR);
    }

    if (dev_fd < 0)
    {
        printf("FrontboardCreate Fbddev Failed\n");
        return -1;
    }
        
    return dev_fd;
}


int uart_read(void *pdata, uint32_t nbytes, int dev_fd)
{
    int read_ret = 0;
    struct timeval timetmp;
    fd_set  fReadfd;
    uint32_t  uiReadLen = 0;
    
    if  (pdata == NULL)
    {
        return -1;
    }
    else if(nbytes <= 0)
    {
        return -2;
    }
    while(uiReadLen < nbytes)
    {
        FD_ZERO(&fReadfd);
        FD_SET(dev_fd, &fReadfd);
        timetmp.tv_sec  = 2;
        timetmp.tv_usec = 0; 
        read_ret = select(dev_fd + 1, &fReadfd, NULL, NULL, &timetmp);
        if ( read_ret > 0)
        {
            read_ret = read(dev_fd, (char*)pdata + uiReadLen, nbytes - uiReadLen);
            if((uint32_t)read_ret != nbytes)
            {
                uiReadLen += read_ret;
            }
            else
            {
                return nbytes;
            }
        }
        else
        {
            break;
        }
    }
    return uiReadLen;
}

int uart_write(void *pdata, uint32_t len, int dev_fd)
{
    int  write_ret  = 0;
    uint32_t uiWriteLen = 0;
    struct timeval timetmp;
    fd_set  fWritefd;

    if  (pdata == NULL)
    {
        printf("Pdata Is NULL\n");
        return -1;
    }
    else if(len <= 0)
    {
        printf("nbytes Must Be > 0\n");
        return -2;
    }
 
    while(uiWriteLen < len)
    {
        FD_ZERO(&fWritefd);
        FD_SET(dev_fd, &fWritefd);
        timetmp.tv_sec  = 0;
        timetmp.tv_usec = 10;      
        write_ret = select(dev_fd + 1, NULL, &fWritefd, NULL, &timetmp);
        if ( write_ret > 0 )
        {
            write_ret = write(dev_fd, (char*)pdata + uiWriteLen, len - uiWriteLen);
            if((uint32_t)write_ret != len)
            {
                uiWriteLen += write_ret;
            }
            else
            {
                return len;
            }
        }
        else
        {
            break;
        }
    }
    return len;
}

int uart_purge(PURGE dw_flags ,int dev_fd)
{
    switch (dw_flags)
    {
        case COMM_PURGE_TXABORT:
            tcflow(dev_fd, TCOOFF);
            break;
        case COMM_PURGE_RXABORT:
            tcflow(dev_fd, TCIOFF);
            break;
        case COMM_PURGE_TXCLEAR:
            tcflush(dev_fd, TCOFLUSH);
            break;
        case COMM_PURGE_RXCLEAR:
            tcflush(dev_fd, TCIFLUSH);
            break;
        default:
            printf("Unknow flag\n");
            return -1;
    }

    return 0;
}

static void init_head_tail(unsigned char* data) {
    data[0] = 0xa5;
    data[1] = 0x5a;
    data[8] = 0xff;
    data[9] = 0xfc; 
}

int ctrl_module_init()
{
    dev_fd = uart_create((char*)RS485_NAME);
    COMM_ATTR comm_attr;
    comm_attr.baudrate = 9600;
    comm_attr.databits = 8;
    comm_attr.stopbits = 1;
    comm_attr.parity = COMM_NOPARITY;
    return uart_set_attribute(dev_fd, &comm_attr);
}

int ctrl_module_close()
{
    return uart_destory(dev_fd);
}

/*Return if write is done.*/
int ctrl_write_ok(unsigned char* txd, int fd)
{
    unsigned char rxd[PROTO_LEN];
    memset(rxd, 0x00, PROTO_LEN);
    uart_read(rxd, PROTO_LEN, fd);
    char msg[128] = {0};
    for (int i=0; i<PROTO_LEN; i++) {
        sprintf(msg+sizeof(msg), "%#x ", rxd[i]);
    }
    return memcmp(txd, rxd, PROTO_LEN);
}

int ctrl_gige_reboot()
{
    __log_mesg("Reboot gige-camera.\n");
    unsigned char txd[PROTO_LEN];
    memset(txd, 0x00, PROTO_LEN);
    init_head_tail(txd);
    txd[2] = 0x02;   
    unsigned char* payload = txd + 3;
    memset(payload, RELAY_ON, RELAY_COUNT);  

    printf("\nSend=>");
    for (int i=0; i<PROTO_LEN; i++) {
        printf("%#x ", txd[i]);
    }
    printf("\n");

    uart_write(txd, PROTO_LEN, dev_fd);
    return ctrl_write_ok(txd, dev_fd);
}


int ctrl_relay_on(int relay)
{
    unsigned char txd[PROTO_LEN];
    unsigned char rxd[PROTO_LEN];
    memset(txd, 0x00, PROTO_LEN);
    memset(rxd, 0x00, PROTO_LEN);
    unsigned char* payload = txd + 3 + relay;
    init_head_tail(txd);
    txd[2] = 0x01;   
    *payload = RELAY_ON;
    printf("\nSend=>");
    for (int i=0; i<PROTO_LEN; i++) {
        printf("%#x ", txd[i]);
    }
    printf("\n");
    uart_write(txd, PROTO_LEN, dev_fd);
    return ctrl_write_ok(txd, dev_fd);
}

int ctrl_relay_off(int relay)
{
    unsigned char txd[PROTO_LEN];
    unsigned char rxd[PROTO_LEN];
    memset(txd, 0x00, PROTO_LEN);
    memset(rxd, 0x00, PROTO_LEN);
    unsigned char* payload = txd + 3 + relay;
    init_head_tail(txd);
    txd[2] = 0x01;   
    *payload = RELAY_OFF;
    
    printf("\nSend=>");
    for (int i=0; i<PROTO_LEN; i++) {
        printf("%#x ", txd[i]);
    }
    printf("\n");

    uart_write(txd, PROTO_LEN, dev_fd);
    return ctrl_write_ok(txd, dev_fd);
}

int ctrl_relay_on_all()
{
    unsigned char txd[PROTO_LEN];
    unsigned char rxd[PROTO_LEN];
    memset(txd, 0x00, PROTO_LEN);
    memset(rxd, 0x00, PROTO_LEN);
    unsigned char* payload = txd + 3;
    init_head_tail(txd);
    txd[2] = 0x01;   
    memset(payload, RELAY_ON, RELAY_COUNT);  
    uart_write(txd, PROTO_LEN, dev_fd);
    return ctrl_write_ok(txd, dev_fd);
}

int ctrl_relay_off_all()
{
    unsigned char txd[PROTO_LEN];
    unsigned char rxd[PROTO_LEN];
    memset(txd, 0x00, PROTO_LEN);
    memset(rxd, 0x00, PROTO_LEN);
    unsigned char* payload = txd + 3;
    init_head_tail(txd);
    txd[2] = 0x01;   
    memset(payload, RELAY_OFF, RELAY_COUNT);  
    uart_write(txd, PROTO_LEN, dev_fd);
    return ctrl_write_ok(txd, dev_fd);
}

void ctrl_test() 
{
    assert(ctrl_module_init() == 0);
    for (int i=0; i<4; i++) {
        assert(ctrl_relay_on(i) == 0);
        sleep(2);
    }    

    for (int i=0; i<4; i++) {
        assert(ctrl_relay_off(i) == 0);
        sleep(2);
    }

    assert(ctrl_relay_on_all() == 0);
    sleep(2);

    assert(ctrl_relay_off_all() == 0);
    sleep(2);

    assert(ctrl_gige_reboot() == 0);
    sleep(2);
    assert(ctrl_module_close() == 0);
    return;  
}
